﻿#ifndef MEPOSE_H
#define MEPOSE_H


#include <iostream>
#include <sstream>
#include <math.h>
#include <string.h>

#include "memath.h"

/**
 * @brief 关节轴的类型
 */
class MeJoints
{
public:
    MeJoints() {
        v[0]=0;
        v[1]=0;
        v[2]=0;
        v[3]=0;
        v[4]=0;
        v[5]=0;
    }
    MeJoints(double j1, double j2, double j3, double j4, double j5, double j6) {
        v[0]=j1;
        v[1]=j2;
        v[2]=j3;
        v[3]=j4;
        v[4]=j5;
        v[5]=j6;
    }
    MeJoints(double value[6]) {
        v[0]=value[0];
        v[1]=value[1];
        v[2]=value[2];
        v[3]=value[3];
        v[4]=value[4];
        v[5]=value[5];
    }
    MeJoints(const double value[6]) {
        v[0]=value[0];
        v[1]=value[1];
        v[2]=value[2];
        v[3]=value[3];
        v[4]=value[4];
        v[5]=value[5];
    }
    double *data() { return v; }
    void zero() {
        memset(v, 0, sizeof (double) * 6);
    }

    bool isValidVal() {
        for (int i = 0; i< 6; i++) {
            if(isnan(v[i]) || isinf(v[i]) ) {
                return false;
            }
        }
        return true;
    }

    double &operator()(unsigned index) {
        return v[index];
    }

    double &operator[](unsigned index) {
        return v[index];
    }

    MeJoints const operator + (MeJoints const &p) {
        MeJoints result;
        for (int i = 0; i < 6; i++) {
            result.v[i] = v[i] + p.v[i];
        }

        return result;
    }

    MeJoints const operator - (MeJoints const &p) {
        MeJoints result;
        for (int i = 0; i < 6; i++) {
            result.v[i] = v[i] - p.v[i];
        }

        return result;
    }

    MeJoints const operator * (double const &scale) {
        MeJoints result;
        for (int i = 0; i < 6; i++) {
            result.v[i] = v[i] * scale;
        }

        return result;
    }

    MeJoints const operator / (double const &scale) {
        MeJoints result;
        for (int i = 0; i < 6; i++) {
            result.v[i] = v[i] / scale;
        }

        return result;
    }

    std::string toString() {
        std::stringstream ss;
        ss << v[0] << ", " << v[1] << ", " << v[2] << ", " << v[3] << ", " << v[4] << ", " << v[5];
        return ss.str();
    }
public:
    double v[6];
};

inline std::ostream & operator<<
(
        std::ostream & stream,
        MeJoints const &jv
        )
{
    stream << jv.v[0] << ", " << jv.v[1] << ", " << jv.v[2] << ", " << jv.v[3] << ", " << jv.v[4] << ", " << jv.v[5];
    return stream;
}

/**
 * @brief Pose
 * x, y, z, u, v, w
 * xyz 单位mm， uvw 三个旋转角，弧度表示
 */
class MePose
{
public:
    MePose() {}
    MePose(double value[6]) {
        fv[0]=value[0];
        fv[1]=value[1];
        fv[2]=value[2];
        fv[3]=value[3];
        fv[4]=value[4];
        fv[5]=value[5];
    }
    MePose(double x, double y, double z, double u, double v, double w) {
        fv[0]=x;
        fv[1]=y;
        fv[2]=z;
        fv[3]=u;
        fv[4]=v;
        fv[5]=w;
    }

    double *data() { return fv; }
    void zero() {
        memset(fv, 0, sizeof (double) * 6);
    }

    bool isZero() {
        if (ME_IS_ZERO(fv[0]) && ME_IS_ZERO(fv[1]) && ME_IS_ZERO(fv[2]) && ME_IS_ZERO(fv[3]) && ME_IS_ZERO(fv[4]) && ME_IS_ZERO(fv[5])) {
            return true;
        }
        return false;
    }
    /// 计算的是 [x, y, z]
    double norm() {
        return sqrt( fv[0] * fv[0] + fv[1] * fv[1] + fv[2] * fv[2] );
    }

    double &operator()(unsigned index) {
        return fv[index];
    }

    double &operator[](unsigned index) {
        return fv[index];
    }

    double operator[](unsigned index) const {
        return fv[index];
    }

    /// 只针对 xyz
    MePose const operator + (MePose const &p) {
        MePose result;
        result.fv[0] = fv[0] + p.fv[0];
        result.fv[1] = fv[1] + p.fv[1];
        result.fv[2] = fv[2] + p.fv[2];

        return result;
    }


    /// 只针对 xyz
    MePose const& operator += (MePose const &p) {
        *this = *this + p;
        return *this;
    }

    /// 只针对 xyz
    MePose const operator - (MePose const &p) {
        MePose result;
        result.fv[0] = fv[0] - p.fv[0];
        result.fv[1] = fv[1] - p.fv[1];
        result.fv[2] = fv[2] - p.fv[2];

        return result;
    }

    /// 只针对 xyz
    MePose const& operator -= (MePose const &p) {
        *this = *this - p;
        return *this;
    }

    /// 只针对 xyz
    MePose const operator * (double const &scale) {
        MePose result;
        result.fv[0] = fv[0] * scale;
        result.fv[1] = fv[1] * scale;
        result.fv[2] = fv[2] * scale;
        result.fv[3] = fv[3];
        result.fv[4] = fv[4];
        result.fv[5] = fv[5];

        return result;
    }

    /// 只针对 xyz
    MePose const& operator *= (double const &scale) {
        *this = *this * scale;
        return *this;
    }

    /// 只针对 xyz
    MePose const operator / (double const &scale) {
        MePose result;
        result.fv[0] = fv[0] / scale;
        result.fv[1] = fv[1] / scale;
        result.fv[2] = fv[2] / scale;

        return result;
    }

    /// 只针对 xyz
    MePose const& operator /= (double const &scale) {
        *this = *this / scale;
        return *this;
    }
public:
    double fv[6];
};

inline std::ostream & operator<<
(
        std::ostream & stream,
        MePose const &p
        )
{
    stream << p.fv[0] << ", " << p.fv[1] << ", " << p.fv[2] << ", " << p.fv[3] << ", " << p.fv[4] << ", " << p.fv[5];
    return stream;
}


#endif // MEPOSE_H
